#include "FingerRootsDetector.h"

#include <opencv2/imgproc.hpp>

FingerRootsDetector::FingerRootsDetector(ImageReader gray)
{
    cv::Mat dx, dy;

    this->ir = gray;
    ir.getImg().copyTo(this->detected_result);

    cv::cvtColor(ir.getImg(), this->gray_tmp, cv::COLOR_BGR2GRAY);
    // cv::blur(this->gray_tmp, this->gray_tmp, cv::Size2d(3, 3));
    cv::Sobel(this->gray_tmp, dx, CV_16S, 1, 0, 3, 1);
    cv::Sobel(this->gray_tmp, dy, CV_16S, 0, 1, 3, 1);
    cv::convertScaleAbs(dx, dx);
    cv::convertScaleAbs(dy, dy);
    cv::addWeighted(dx, 0.5, dy, 0.5, 0, this->gray_tmp);
    this->gray_tmp.convertTo(this->gray_tmp, CV_8UC1);
    cv::adaptiveThreshold(this->gray_tmp, this->gray_tmp, 255, cv::ADAPTIVE_THRESH_MEAN_C, cv::THRESH_BINARY, 63, -5);
    cv::erode(this->gray_tmp, this->gray_tmp, cv::Mat(), cv::Point2d(-1, -1), 1);
    cv::dilate(this->gray_tmp, this->gray_tmp, cv::Mat(), cv::Point2d(-1, -1), 1);

    // debug
    // this->detected_result = this->gray_tmp;
}

FingerRootsDetector::~FingerRootsDetector()
{
}

bool FingerRootsDetector::detectLR()
{
    //Left return true,right return false
    if (this->ir.path.find("LLL") != string::npos)
    {
        return true;
    }
    else if (this->ir.path.find("RRR") != string::npos)
    {
        return false;
    }
    else
    {
        throw exception();
    }
}

FingerRootsDetector FingerRootsDetector::detect()
{
    this->isLeftHand = this->detectLR();
    this->findFingerRoots(this->getCornerPoints());
    cv::circle(this->detected_result, cv::Point(this->fingerConner[0], this->fingerConner[1]), 5, cv::Scalar(0, 255, 0), 2, 8, 0);
    cv::circle(this->detected_result, cv::Point(this->fingerConner[2], this->fingerConner[3]), 5, cv::Scalar(0, 255, 0), 2, 8, 0);
    cv::circle(this->detected_result, cv::Point(this->fingerConner[4], this->fingerConner[5]), 5, cv::Scalar(0, 255, 0), 2, 8, 0);
    return *this;
}

const int FingerRootsDetector::getDetectedArr(int *&result)
{
    /*
    if (this->fingerConner[0] > this->fingerConner[1] || this->fingerConner[2] > this->fingerConner[3] || fingerConner[4] > fingerConner[5])
    {
        return 0;
    }
    */
    for (int i = 0; i < 6; i++)
    {
        result[i] = this->fingerConner[i];
    }

    return 1;
}

vector<cv::Point> FingerRootsDetector::getCornerPoints()
{
    int num = 3;
    const int thresh[3] = {160, 160, 160};

    cv::Mat palm, detectedCorner;
    cv::Mat normImage[3];
    vector<cv::Point> corners(10);

    palm = this->gray_tmp;
    cv::erode(palm, palm, cv::Mat());
    cv::dilate(palm, palm, cv::Mat());

    cv::cornerHarris(palm, detectedCorner, 4, 3, 0.04); //small corner
    cv::normalize(detectedCorner, normImage[0], 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
    cv::cornerHarris(palm, detectedCorner, 20, 11, 0.04); //middle corner
    cv::normalize(detectedCorner, normImage[1], 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
    cv::cornerHarris(palm, detectedCorner, 30, 19, 0.04); //big corner
    cv::normalize(detectedCorner, normImage[2], 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
    for (int k = 0; k < 3; k++)
        for (int j = 0; j < normImage[k].rows; j++)
        {
            for (int i = 0; i < normImage[k].cols; i++)
            {
                if ((int)normImage[k].at<float>(j, i) > thresh[k])
                {
                    cv::Point tmp(i, j);
                    corners.push_back(tmp);
                    // debug
                    // cv::circle(this->detected_result, tmp, 5, cv::Scalar(80), 2, 8, 0);
                }
            }
        }
    return corners;
}

void FingerRootsDetector::findFingerRoots(vector<cv::Point> candidator)
{
    cv::Point result[3];

    vector<cv::Point> candidator_filtered = this->filterFingerRoots_aggregate(candidator);
    vector<cv::Point> candidator_result = this->filterFingerRoots_distance(candidator);

    for (int i = 0; i < 3; i++)
    {
        result[i] = candidator_result[i];
    }
    //finger roots [x1,y1,x2,y2,x3,y3]
    this->fingerConner[0] = result[0].x;
    this->fingerConner[1] = result[0].y;
    this->fingerConner[2] = result[1].x;
    this->fingerConner[3] = result[1].y;
    this->fingerConner[4] = result[2].x;
    this->fingerConner[5] = result[2].y;
}

float FingerRootsDetector::distance(cv::Point a, cv::Point b)
{
    return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
}

vector<cv::Point> FingerRootsDetector::filterFingerRoots_aggregate(vector<cv::Point> candidator)
{
    const int threshold = 16;
    const int patch_col = (this->detected_result.rows + 63) / 64;
    const int patch_row = (this->detected_result.cols + 63) / 64;
    vector<cv::Point> patch[patch_row][patch_col];
    vector<cv::Point> result;

    for (int i = 0; i < patch_row; i++)
    {
        for (int j = 0; j < patch_col; j++)
        {
            patch[i][j] = vector<cv::Point>();
        }
    }

    for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
    {
        int x = i->x / 64;
        int y = i->y / 64;
        patch[x][y].push_back(*i);
    }

    for (int i = 0; i < patch_row; i++)
    {
        for (int j = 0; j < patch_col; j++)
        {
            if (patch[i][j].size() < threshold)
            {
                continue;
            }
            for (vector<cv::Point>::iterator k = patch[i][j].begin(); k != patch[i][j].end(); k++)
            {
                result.push_back(*k);
                // debug
                cv::circle(this->detected_result, *k, 5, cv::Scalar(80), 2, 8, 0);
            }
        }
    }

    return result;
}

vector<cv::Point> FingerRootsDetector::filterFingerRoots_distance(vector<cv::Point> candidator)
{
    int first_middle_ = 99999;
    int middle_ring_ = 99999;
    int ring_little_ = 99999;
    int d[3] = {0};
    cv::Point result[3];
    vector<cv::Point> result_vector;

    //right hand roots pre-knowledges
    // cv::Point f_m_p(210, 295);
    // cv::Point m_r_p(210, 220);
    // cv::Point r_l_p(240, 150);
    cv::Point f_m_p(420, 190);
    cv::Point m_r_p(505, 200);
    cv::Point r_l_p(605, 240);
    if (this->isLeftHand)
    {
        //left hand roots pre-knowledges
        // f_m_p = cv::Point(220, 185);
        // m_r_p = cv::Point(210, 260);
        // r_l_p = cv::Point(240, 330);
        f_m_p = cv::Point(480, 190);
        m_r_p = cv::Point(380, 215);
        r_l_p = cv::Point(285, 240);
    }
    // debug
    cv::circle(this->detected_result, f_m_p, 5, cv::Scalar(0, 0, 255), 2, 8, 0);
    cv::circle(this->detected_result, m_r_p, 5, cv::Scalar(0, 0, 255), 2, 8, 0);
    cv::circle(this->detected_result, r_l_p, 5, cv::Scalar(0, 0, 255), 2, 8, 0);

    for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
    {
        d[0] = distance(*i, f_m_p);
        d[1] = distance(*i, m_r_p);
        d[2] = distance(*i, r_l_p);

        if (d[0] < first_middle_)
        {
            result[0] = *i;
            first_middle_ = d[0];
        }
        if (d[1] < middle_ring_)
        {
            result[1] = *i;
            middle_ring_ = d[1];
        }
        if (d[2] < ring_little_)
        {
            result[2] = *i;
            ring_little_ = d[2];
        }
    }

    result_vector.push_back(result[0]);
    result_vector.push_back(result[1]);
    result_vector.push_back(result[2]);

    return result_vector;
}